
#include "util.h"

#include <math.h>

#include "vector4.h"


#define SMALL_NUM 0.00000001

bool Util::segmentPlaneCollision( Vector4 *segmentP1, Vector4 *segmentP2,
							Vector4 *planeP1, Vector4 *planeN, Vector4 *i)
{
	//std::cout << "u=planeP - segmentP1" << std::endl;
    Vector4   u = segmentP2->minus(segmentP1);
    Vector4   w = segmentP1->minus(planeP1);

    float     N = -1.f * planeN->dot(w);
	float     D = planeN->dot(u);

    if (D == 0) {          // segment is parallel to plane
		//std::cout << " false, parallel!" << std::endl;
		return false;                   // no intersection
    }
    // they are not parallel
    // compute intersect param
    float sI = N / D;
	//std::cout << " sI= N / D: " << sI << " = " << N << " / " << D << std::endl;
	if (sI < 0 || sI > 1){
		//std::cout << " false, segment doesn't hit!" << std::endl;
        return false;                       // no intersection
	}

    *i = segmentP1->plus(u * sI);             // compute segment intersect point
    return true;
}

Vector4 Util::getNormal(Vector4 *A, Vector4 *B, Vector4 *C){
	Vector4 AB = B->minus(A);
	Vector4 AC = C->minus(A);
	//std::cout << "-AB: " << AB[0] << " " << AB[1] << " " << AB[2] << std::endl;
	//std::cout << "-AC: " << AC[0] << " " << AC[1] << " " << AC[2] << std::endl;
	Vector4 returnVec = AB.cross(AC);
	returnVec.normalize();
	//std::cout << "--rVec: " << returnVec[0] << " " << returnVec[1] << " " << returnVec[2] << std::endl;
	return returnVec;
}

float Util::CalcAngleSum(Vector4 *point,Vector4 **vertices,int n)
{
   float TWOPI = 6.28318530717958647f;
   int i = 0;
   float m1 = 0.0f,m2 = 0.0f;
   float anglesum=0.0f,costheta = 0.0f;
   Vector4 p1 = Vector4();
   Vector4 p2 = Vector4();

   for (i=0;i<n;i++) {

      p1[0] = vertices[i]->get(0) - point->get(0);
      p1[1] = vertices[i]->get(1) - point->get(1);
      p1[2] = vertices[i]->get(2) - point->get(2);
      p2[0] = vertices[(i+1)%n]->get(0) - point->get(0);
      p2[1] = vertices[(i+1)%n]->get(1) - point->get(1);
      p2[2] = vertices[(i+1)%n]->get(2) - point->get(2);

      m1 = p1.modulus();
      m2 = p2.modulus();
      if (m1*m2 <= 0.0000001)
         return(TWOPI); /* We are on a node, consider this inside */
      else
         costheta = (p1[0]*p2[0] + p1[1]*p2[1] + p1[1]*p2[1]) / (m1*m2);

      anglesum += acos(costheta);
   }
   return(anglesum);
}

bool Util::doesParticleIntersectBox(Particle *point, Body *b){
	Vector4 **vertices = b->getFaceVerts();
	for (int i = 0; i < 6; i++){
		Vector4 planeNormal = Util::getNormal(vertices[(4*i)+0], vertices[(4*i)+2], vertices[(4*i)+1]);
		Vector4 *intersection = new Vector4();
		bool mayColPlane = Util::segmentPlaneCollision(&(point->pos), &(point->lastPos), vertices[(4*i)+0], &(planeNormal), intersection);
		mayColPlane = mayColPlane || Util::segmentPlaneCollision(&(point->lastPos), &(point->pos), vertices[(4*i)+0], &(planeNormal), intersection);
		if (mayColPlane == true){
			Vector4 *planeVerts[4];
			for (int j = 0; j < 4; j++){
				planeVerts[j] = vertices[(4*i)+j];
			}
			float colPlane = Util::CalcAngleSum(intersection, planeVerts, 4);
			//std::cout << "colFloor - " << colFloor << std::endl;
			if (colPlane >= 6.0f && colPlane <= 6.6f){
				return true;
			}
		}

	}
	return false;
}
